#include "Driver.h"

Driver::Driver():m_loop_rate(10000){

	// set local_var speed
	m_current_speed = 0;

	// set speed
	m_velCommand.linear.x = 0;
	m_velCommand.linear.y = 0;
	m_velCommand.linear.z = 0;

	// set direction
	m_velCommand.angular.x = 0;
	m_velCommand.angular.y = 0;
	m_velCommand.angular.z = 0;

	//Setting up the publisher to cmd_vel topic
	m_roverMdlPub = m_nodeHandler.advertise<geometry_msgs::Twist>("cmd_vel",1000);
}

Driver::Driver(double speed, double direction):m_loop_rate(10000){

	// set local_var speed
	m_current_speed = speed;

	// set speed
	m_velCommand.linear.x = speed;
	m_velCommand.linear.y = 0;
	m_velCommand.linear.z = 0;

	// set direction
	m_velCommand.angular.x = 0;
	m_velCommand.angular.y = 0;
	m_velCommand.angular.z = direction;

	//Setting up the publisher to cmd_vel topic
	m_roverMdlPub = m_nodeHandler.advertise<geometry_msgs::Twist>("cmd_vel",1000);
}

void Driver::move(double speed, double direction) {

	// set local_var speed
	m_current_speed = speed;

	// set speed
	m_velCommand.linear.x = speed;
	m_velCommand.linear.y = 0;
	m_velCommand.linear.z = 0;

	// set direction
	m_velCommand.angular.x = 0;
	m_velCommand.angular.y = 0;
	m_velCommand.angular.z = direction;

//TODO check why it twice
	m_roverMdlPub.publish(m_velCommand);
	m_loop_rate.sleep();
	m_roverMdlPub.publish(m_velCommand);
}

void Driver::turn(double direction) {

	// set speed
	m_velCommand.linear.x = m_current_speed;
	m_velCommand.linear.y = 0;
	m_velCommand.linear.z = 0;

	// set direction
	m_velCommand.angular.x = 0;
	m_velCommand.angular.y = 0;
	m_velCommand.angular.z = direction;

//TODO check why it twice
	m_roverMdlPub.publish(m_velCommand);
	m_loop_rate.sleep();
	m_roverMdlPub.publish(m_velCommand);
}

void Driver::stop() {

	// set 0 velocity
	move(0.0, 0.0);
}
